#include "../axisobject.h"
#include "../JointDecouple/dataanalysizerdouble.h"
int testForAxisObjectOnePoint()
{

    JointAttribute attribute;
    attribute.coulombFriction=0.2;
    attribute.coupleFlag=0;
    attribute.coupleJoint=0;
    attribute.coupleReducer=0;
    for(int i=0;i<4;i++)
    {
        attribute.dhParameter[i]=i;
    }
    attribute.dhType=0;
    attribute.encoderResolution=8;
    attribute.pulsePerRound=10000;
    attribute.externAxisFlag=0;
    attribute.gearRatio=50;
    attribute.jointName="J1";
    attribute.mass=2.2;
    //attribute.massCenter=0.0;
    attribute.maxVelocity=10;
    attribute.maxAccelation=2;

    attribute.maxPosition=50000;
    attribute.maxTorque=120;

    attribute.minPosition=-100;
    attribute.mobile=0;
    attribute.positionOffset=25;
    attribute.processFollowError=50;
    attribute.screwPitch=25;
    attribute.targetFollowError=0.4;
    attribute.viscousFriction=0.14;

    AxisObject axisObject(attribute);

    //测试trapMove
    //axisObject.trapMove(340,1);
    //axisObject.trapMoveByTime(500,18);
    //axisObject.splineZeroMove(3550,1);
    //axisObject.splineZeroMoveByTime(466,220);
    axisObject.splineContinueMove(399,56,16);

    double position;
    double vel;
    double acc;
    double torque;
    std::vector<double> axisData;
    axisData.resize(4);
    DataAnalysizerDouble dataAnalysizer("axis data analysizer!!!\n",1,1000,4,10000,0);
     while(1==axisObject.moveOneStep())
     {
    axisObject.returnAxisCommandData(position, vel ,acc, torque);
    axisData[0]=position;
    axisData[1]=vel;
    axisData[2]=acc;
    axisData[3]=torque;
    dataAnalysizer.recordDataForAnalysize(axisData);
     }



     dataAnalysizer.writeDataQueueToFile("SingleAxisData.csv");

     return 1;
}

int testForAxisObject()
{
    JointAttribute attribute;
    attribute.coulombFriction=0.2;
    attribute.coupleFlag=0;
    attribute.coupleJoint=0;
    attribute.coupleReducer=0;
    for(int i=0;i<4;i++)
    {
        attribute.dhParameter[i]=i;
    }
    attribute.dhType=0;
    attribute.encoderResolution=8;
    attribute.pulsePerRound=10000;
    attribute.externAxisFlag=0;
    attribute.gearRatio=50;
    attribute.jointName="J1";
    attribute.mass=2.2;
    //attribute.massCenter=0.0;
    attribute.maxVelocity=10;
    attribute.maxAccelation=2;

    attribute.maxPosition=500;
    attribute.maxTorque=120;

    attribute.minPosition=-100;
    attribute.mobile=0;
    attribute.positionOffset=25;
    attribute.processFollowError=50;
    attribute.screwPitch=25;
    attribute.targetFollowError=0.4;
    attribute.viscousFriction=0.14;

    AxisObject axisObject(attribute);

    //测试trapMove
    //axisObject.trapMove(340,1);
    //axisObject.trapMoveByTime(500,99);
    //axisObject.splineZeroMove(355,1);
    //axisObject.splineZeroMoveByTime(466,220);
    axisObject.splineContinueMove(399,56,6);

    double position;
    double vel;
    double acc;
    double torque;
    std::vector<double> axisData;
    axisData.resize(4);
    DataAnalysizerDouble dataAnalysizer("axis data analysizer!!!\n",1,1000,4,10000,0);
     while(1==axisObject.moveOneStep())
     {
    axisObject.returnAxisCommandData(position, vel ,acc, torque);
    axisData[0]=position;
    axisData[1]=vel;
    axisData[2]=acc;
    axisData[3]=torque;
    dataAnalysizer.recordDataForAnalysize(axisData);
     }

     axisObject.splineContinueMove(-230,60,-58);

     while(1==axisObject.moveOneStep())
     {
    axisObject.returnAxisCommandData(position, vel ,acc, torque);
    axisData[0]=position;
    axisData[1]=vel;
    axisData[2]=acc;
    axisData[3]=torque;
    dataAnalysizer.recordDataForAnalysize(axisData);
     }


     dataAnalysizer.writeDataQueueToFile("SingleAxisData.csv");

     return 1;
}

